#include "main.hpp"
#include "logic.hpp"
#include "graph.hpp"
#include "initialize.hpp"
#include "json/json.h"
#include "config.hpp"
#include <fstream>

int main(int argc, char *argv[])
{
	int mode = 19;
	ifstream ifs;
	ifs.open("config.json");
	assert(ifs.is_open());
	Json::Reader reader;
	Json::Value root;

	if(!reader.parse(ifs,root,false)){
		cout<<"parse error"<<strerror(errno)<<endl;
		return -1;
	}
	int size = root.size();
	// assert(size>=30);
	cout<<"load config,"<<size<<" elements in total"<<endl;

	camIndexA = root["vision"]["camA"].asInt();
	camIndexB = root["vision"]["camB"].asInt();
	cfgname = root["vision"]["cfg"].asString();
	wtsname = root["vision"]["weights"].asString();
	camdataname = root["vision"]["camData"].asString();
	TARGETSIZE = root["logic_index"]["targetSize"].asInt();
	MINSIZETOCONTROL = root["logic_index"]["minSize2Ctrl"].asInt();
	MAXSIZETOCONTROL = root["logic_index"]["maxSize2Ctrl"].asInt();
	xp=root["control_index"]["xp"].asFloat();
	xi=root["control_index"]["xi"].asFloat();
	xd=root["control_index"]["xd"].asFloat();
	yp=root["control_index"]["rp"].asFloat();
	yi=root["control_index"]["ri"].asFloat();
	yd=root["control_index"]["rd"].asFloat();
	lmp=root["control_index"]["lmp"].asFloat();
	lmi=root["control_index"]["lmi"].asFloat();
	lmd=root["control_index"]["lmd"].asFloat();
	xq=root["control_index"]["xq"].asFloat();
	xr=root["control_index"]["xr"].asFloat();
	yq=root["control_index"]["yq"].asFloat();
	yr=root["control_index"]["yr"].asFloat();
	wq=root["control_index"]["wq"].asFloat();
	wr=root["control_index"]["wr"].asFloat();
	hq=root["control_index"]["hq"].asFloat();
	hr=root["control_index"]["hr"].asFloat();
	yawOver=root["logic_index"]["firstTurnTo"].asInt();
	yawthrott=root["logic_index"]["yawthrott"].asFloat();
	xfast=root["logic_index"]["xfast"].asInt();
	xslow=root["logic_index"]["xslow"].asInt();
	xmedium=root["logic_index"]["xmedium"].asInt();
	BALLID = root["logic_index"]["ballcolor"].asInt();
	qualifiedBall = root["logic_index"]["qualifiedBall"].asInt();
	qualifiedLandmark = root["logic_index"]["qualifiedLandmark"].asInt();
	qualifiedFrame = root["logic_index"]["qualifiedFrame"].asInt();
	maxBall = root["logic_index"]["collidedBall"].asInt();
	choosePath = root["logic_index"]["choosePath"].asInt();
	mode = root["control_index"]["mode"].asInt();
	xbias = root["control_index"]["xbias"].asInt();
	ybias = root["control_index"]["ybias"].asInt();
	zbias = root["control_index"]["zbias"].asInt();
	rbias = root["control_index"]["rbias"].asInt();
	deepbias = root["control_index"]["deepbias"].asFloat();
	func0(mode);	
	return 0;
}
